﻿/*
 * Author: Gabriel Reiser
 * Contributors:
 */

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Irrlicht.Net.Core
{
    [Serializable]
    public struct Matrix3D
    {
        //internal float[,] fields;
        public float m11, m12, m13,
                     m21, m22, m23,
                     m31, m32, m33;

        private readonly static Matrix3D zero = new Matrix3D(
                        0, 0, 0,
                        0, 0, 0,
                        0, 0, 0);
        private readonly static Matrix3D identity = new Matrix3D(
                1, 0, 0,
                0, 1, 0,
                0, 0, 1);

        

        public Vector3D Scale
        {
            get
            {
                return new Vector3D(this.m11, this.m22, this.m33);
            }
            set
            {
                this.m11 = value.x;
                this.m22 = value.y;
                this.m33 = value.z;
            }
        }

        public float Determinant
        {
            get
            {
                return m11 * (m22 * m33 - m23 * m32) +
                       m12 * (m23 * m31 - m21 * m33) +
                       m13 * (m21 * m32 - m22 * m31);
            }
        }

        public Vector3D XAxis { get { return new Vector3D(m11, m21, m31); } set { m11 = value.x; m21 = value.y; m31 = value.z; } }
        public Vector3D YAxis { get { return new Vector3D(m12, m22, m32); } set { m12 = value.x; m22 = value.y; m32 = value.z; } }
        public Vector3D ZAxis { get { return new Vector3D(m13, m23, m33); } set { m13 = value.x; m23 = value.y; m33 = value.z; } }

        public Matrix3D Zero { get { return zero; } }
        public Matrix3D Identity { get { return identity; } }
        
        
        public Matrix3D(float m1_1, float m2_1, float m3_1, 
                      float m1_2, float m2_2, float m3_2, 
                      float m1_3, float m2_3, float m3_3)
        {
            m11 = m1_1; m12 = m1_2; m13 = m1_3;
            m21 = m2_1; m22 = m2_2; m23 = m2_3;
            m31 = m3_1; m32 = m3_2; m33 = m3_3;
            
        }
        public Matrix3D(Vector3D XAxis, Vector3D YAxis, Vector3D ZAxis)
        {
            m11 = XAxis.x; m12 = YAxis.x; m13 = ZAxis.x;
            m21 = XAxis.y; m22 = YAxis.y; m23 = ZAxis.y;
            m31 = XAxis.z; m32 = YAxis.z; m33 = ZAxis.z;
            
        }
        public static Matrix3D FromAxis(Vector3D XAxis, Vector3D YAxis, Vector3D ZAxis)
        {
            return new Matrix3D(XAxis, YAxis, ZAxis);
        }

        public void FromEulerAnglesDegrees(float yaw, float pitch, float roll)
        {
            float cos = (float)Math.Cos(yaw);
            float sin = (float)Math.Sin(yaw);
            Matrix3D xMat = new Matrix3D(1, 0, 0, 0, cos, -sin, 0, sin, cos);

            cos = (float)Math.Cos(pitch);
            sin = (float)Math.Sin(pitch);
            Matrix3D yMat = new Matrix3D(cos, 0, sin, 0, 1, 0, -sin, 0, cos);

            cos = (float)Math.Cos(roll);
            sin = (float)Math.Sin(roll);
            Matrix3D zMat = new Matrix3D(cos, -sin, 0, sin, cos, 0, 0, 0, 1);

            this = xMat * (yMat * zMat);
        }

        public void FromEulerAnglesRadians(float yaw, float pitch, float roll)
        {
            yaw = yaw * (float)(180.0 / Math.PI);
            pitch = pitch * (float)(180.0 / Math.PI);
            roll = roll * (float)(180.0 / Math.PI);

            FromEulerAnglesDegrees(yaw, pitch, roll);
        }

        public static Matrix3D operator *(Matrix3D val1, Matrix3D val2)
        {
            Matrix3D result = new Matrix3D();

            result.m11 = val1.m11 * val2.m11 + val1.m12 * val2.m21 + val1.m13 * val2.m31;
            result.m12 = val1.m11 * val2.m12 + val1.m12 * val2.m22 + val1.m13 * val2.m32;
            result.m13 = val1.m11 * val2.m13 + val1.m12 * val2.m23 + val1.m13 * val2.m33;
            

            result.m21 = val1.m21 * val2.m11 + val1.m22 * val2.m21 + val1.m23 * val2.m31;
            result.m22 = val1.m21 * val2.m12 + val1.m22 * val2.m22 + val1.m23 * val2.m32;
            result.m23 = val1.m21 * val2.m13 + val1.m22 * val2.m23 + val1.m23 * val2.m33;
            

            result.m31 = val1.m31 * val2.m11 + val1.m32 * val2.m21 + val1.m33 * val2.m31;
            result.m32 = val1.m31 * val2.m12 + val1.m32 * val2.m22 + val1.m33 * val2.m32;
            result.m33 = val1.m31 * val2.m13 + val1.m32 * val2.m23 + val1.m33 * val2.m33;
            

            

            return result;

        }

        public static Vector3D operator *(Matrix3D matrix, Vector3D vector)
        {
            Vector3D result = new Vector3D();

            result.x = ((matrix.m11 * vector.x) + (matrix.m12 * vector.y) + (matrix.m13 * vector.z));
            result.y = ((matrix.m21 * vector.x) + (matrix.m22 * vector.y) + (matrix.m23 * vector.z));
            result.z = ((matrix.m31 * vector.x) + (matrix.m32 * vector.y) + (matrix.m33 * vector.z));

            return result;
        }

        public static Matrix3D operator *(Matrix3D val1, float scalar)
        {
            Matrix3D result = new Matrix3D();

            result.m11 = val1.m11 * scalar;
            result.m12 = val1.m12 * scalar;
            result.m13 = val1.m13 * scalar;
            

            result.m21 = val1.m21 * scalar;
            result.m22 = val1.m22 * scalar;
            result.m23 = val1.m23 * scalar;
            

            result.m31 = val1.m31 * scalar;
            result.m32 = val1.m32 * scalar;
            result.m33 = val1.m33 * scalar;
            
            return result;
        }
        public static Matrix3D operator +(Matrix3D val1, Matrix3D val2)
        {
            Matrix3D result = new Matrix3D();

            result.m11 = val1.m11 + val2.m11;
            result.m12 = val1.m12 + val2.m12;
            result.m13 = val1.m13 + val2.m13;
            

            result.m21 = val1.m21 + val2.m21;
            result.m22 = val1.m22 + val2.m22;
            result.m23 = val1.m23 + val2.m23;
            

            result.m31 = val1.m31 + val2.m31;
            result.m32 = val1.m32 + val2.m32;
            result.m33 = val1.m33 + val2.m33;
            

            return result;
        }
        public static Matrix3D operator -(Matrix3D val1, Matrix3D val2)
        {
            Matrix3D result = new Matrix3D();

            result.m11 = val1.m11 - val2.m11;
            result.m12 = val1.m12 - val2.m12;
            result.m13 = val1.m13 - val2.m13;
            

            result.m21 = val1.m21 - val2.m21;
            result.m22 = val1.m22 - val2.m22;
            result.m23 = val1.m23 - val2.m23;
            

            result.m31 = val1.m31 - val2.m31;
            result.m32 = val1.m32 - val2.m32;
            result.m33 = val1.m33 - val2.m33;
            
            return result;
        }
        public static Matrix3D operator -(Matrix3D val1)
        {
            Matrix3D result = new Matrix3D();
            result.m11 = -val1.m11;
            result.m12 = -val1.m12;
            result.m13 = -val1.m13;
            
            result.m21 = -val1.m21;
            result.m22 = -val1.m22;
            result.m23 = -val1.m23;
            
            result.m31 = -val1.m31;
            result.m32 = -val1.m32;
            result.m33 = -val1.m33;
            
            return result;
        }
        public Matrix3D Inverse()
        {
            Matrix3D inv = new Matrix3D(this.XAxis, this.YAxis, this.ZAxis);
            float fDet = Determinant;
            if ( Math.Abs(fDet) <= float.Epsilon )
                return this;

            float InvDet = 1.0f/fDet;
            for (int Row = 0; Row < 3; Row++)
            {
                for (int Col = 0; Col < 3; Col++)
                {
                    inv[Row, Col] *= InvDet;
                }
            }

            return inv;
        }
        public static bool operator ==(Matrix3D val1, Matrix3D val2)
        {
            if (
                    val1.m11 == val2.m11 && val1.m12 == val2.m12 && val1.m13 == val2.m13 &&
                    val1.m21 == val2.m21 && val1.m22 == val2.m22 && val1.m23 == val2.m23 &&
                    val1.m31 == val2.m31 && val1.m32 == val2.m32 && val1.m33 == val2.m33)
                return true;

            return false;
        }
        public static bool operator !=(Matrix3D val1, Matrix3D val2)
        {
            return !(val1 == val2);
        }

        public Matrix3D Transpose()
        {
            return new Matrix3D(this.m11, this.m21, this.m31,
                                this.m12, this.m22, this.m32,
                                this.m13, this.m23, this.m33);
        }
        

        public override bool Equals(object obj)
        {
            if (obj is Matrix3D)
                return (this == (Matrix3D)obj);
            else
                return false;
        }

        public override int GetHashCode()
        {
            return (int)(m11 + m12 + m13 + m21 + m22 + m23 + m31 + m32 + m33);
        }
        
        public float this[int row, int col]
        {
            get
            {
                unsafe
                {
                    fixed (float* pM = &m11)
                        return *(pM + ((3 * row) + col));
                }
            }
            set
            {
                unsafe
                {
                    fixed (float* pM = &m11)
                        *(pM + ((3 * row) + col)) = value;
                }
            }
        }

        public float this[int index]
        {
            get
            {
                unsafe
                {
                    fixed (float* pMatrix = &m11)
                    {
                        return *(pMatrix + index);
                    }
                }
            }
            set
            {
                unsafe
                {
                    fixed (float* pMatrix = &m11)
                    {
                        *(pMatrix + index) = value;
                    }
                }
            }
        } 
        /*public static Matrix CreateRotationX(float rotation)
        {

        }*/
    }
}
